#include <inttypes.h>
#include <pthread.h>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>

#include "lcm/lcm.h"

#include "common/getopt.h"
#include "common/timestamp.h"

//////////////////////////////////////////////////////////////////////
//////////////////////////////// Structs /////////////////////////////
//////////////////////////////////////////////////////////////////////
typedef struct spyinfo spyinfo_t;
struct spyinfo
{
    GHashTable *hashtable;
    pthread_mutex_t mutex;
    volatile int64_t quit;
    float display_hz;
};


typedef struct msg_info msg_info_t;
struct msg_info
{
    int num_msgs;
    int64_t last_utime;
    float hz_ave;  // exponential average
};

//////////////////////////////////////////////////////////////////////
////////////////////////// Helper Functions //////////////////////////
//////////////////////////////////////////////////////////////////////
static void cleanup(int s, void *arg)
{
    spyinfo_t *spy = (spyinfo_t *)arg;
    spy->quit = 1;
    printf("done\n");
}

static void sighandler(int s)
{
	switch(s) {
        case SIGQUIT:
        case SIGINT:
        case SIGTERM:
            printf("Exiting...");
            exit(0);
        default:
            break;
	}
}

// XXX temporary until ncurses installed on pandas
void clearscreen() {
    system( "clear" );
}

//////////////////////////////////////////////////////////////////////
///////////////////////////// LCM HANDLER ////////////////////////////
//////////////////////////////////////////////////////////////////////
// don't use this lcm handler because you don't know how to cast the
// messages into anything desired types
void handler_all_lcm (const lcm_recv_buf_t *rbuf,
                      const char *channel, void *arg)
{
    spyinfo_t *spy = (spyinfo_t *)arg;
    msg_info_t *minfo;

    int64_t utime = timestamp_now();

    pthread_mutex_lock(&spy->mutex);
    {
        minfo = (msg_info_t *) g_hash_table_lookup(spy->hashtable, channel);
        if (minfo == NULL) {
            if ((minfo = (msg_info_t *) calloc(1, sizeof(msg_info_t))) == NULL)
                return;
            minfo->num_msgs = 0;
            g_hash_table_insert(spy->hashtable, channel, minfo);
        }

        if (minfo->last_utime > 0) {
            int64_t dt = utime - minfo->last_utime;
            if (dt > 0)
                minfo->hz_ave = (minfo->num_msgs*minfo->hz_ave + 1000000.0 / dt) / (minfo->num_msgs + 1);
        } else
            minfo->hz_ave = 0;
    }
    pthread_mutex_unlock(&spy->mutex);
    minfo->num_msgs++;
    minfo->last_utime = timestamp_now();

}

static void print_msg_info(gpointer key, gpointer value, gpointer user_data)
{

    msg_info_t *minfo = (msg_info_t *)value;
    int64_t *utime = (int64_t *)user_data;

    float dt = ((*utime) - minfo->last_utime) / 1000000.0;

    // XXX if we haven't gotten a message for (something like) 10 periods (expected) lost contact
    if (minfo->hz_ave > 0 && dt > 10 / minfo->hz_ave) {
        minfo->hz_ave = 0;
    }

    printf("%24s\t%12d\t%8.2f\t%7.4f\n", (char *)key, minfo->num_msgs, minfo->hz_ave, dt);
}

//////////////////////////////////////////////////////////////////////
//////////////////////////// Print Thread ////////////////////////////
//////////////////////////////////////////////////////////////////////
void *print_thread(void *arg)
{
    spyinfo_t *spy = (spyinfo_t *)arg;

    const double MAX_FREQ = 100.0;
    int period;
    float hz = 10.0;

    if (spy->display_hz <= 0) {
        printf("WRN: Invalid Display Hz, defaulting to %3.3fHz\n", hz);
    } if (spy->display_hz > MAX_FREQ) {
        printf("WRN: Invalid Display Hz, defaulting to %1.0f Hz\n", MAX_FREQ);
        hz = MAX_FREQ;
    } else
        hz = spy->display_hz;

    period = 1000000 / hz;

    printf("NFO: %s: Starting\n", "print_thread");
    while (!spy->quit) {
        usleep(period);

        clearscreen();
        printf("********************************************************************************\n");
        printf("*************************** LCM-SPY (lite) [%3.1f Hz] ***************************\n", hz);
        printf("********************************************************************************\n");
        printf("%24s\t%12s\t%8s\t%12s\n", "Channel", "Num Messages", "Hz (ave)", "Last Msg (s)");

        int64_t utime = timestamp_now();
        pthread_mutex_lock(&spy->mutex);
        {
            g_hash_table_foreach(spy->hashtable, print_msg_info, &utime);
        }
        pthread_mutex_unlock(&spy->mutex);
    }
    printf("NFO: %s: Ending\n", "print_thread");

    return NULL;
}

//////////////////////////////////////////////////////////////////////
////////////////////////////////// MAIN //////////////////////////////
//////////////////////////////////////////////////////////////////////
int main(int argc, char *argv[])
{
    // getopt setup
    getopt_t *gopt = getopt_create();
    getopt_add_string(gopt, 'u', "url", "udpm://239.255.76.67:7667?ttl=0", "LCM url");
    getopt_add_int(gopt, 'f', "freq", "10", "Display Hz");

    if (!getopt_parse(gopt, argc, argv, 1)) {
        printf("Options:\n");
        getopt_do_usage(gopt);
        return 0;
    }

    spyinfo_t spy = { .quit = 0, .hashtable = g_hash_table_new(g_str_hash, g_str_equal),
                      .display_hz = getopt_get_int(gopt, "freq")};
    if (spy.hashtable == NULL) {
        printf("spyinfo");
        exit(-1);
    }

    // lcm setup
    char *url = getopt_get_string(gopt, "url");
    lcm_t *lcm = lcm_create(url);
    lcm_subscription_t *lcm_all = lcm_subscribe(lcm, ".*", handler_all_lcm, &spy);

	on_exit(cleanup, &spy);
	signal(SIGINT, sighandler);
	signal(SIGQUIT, sighandler);
	signal(SIGTERM, sighandler);

    // start threads
    pthread_mutex_init(&spy.mutex, NULL);
    pthread_t pid;
    if (pthread_create(&pid, NULL, (void *) print_thread, &spy)) {
        printf("ERR: %s: Failed to start thread\n", "print_thread");
        exit(-1);
    }

    while(!spy.quit) {
        if (lcm_handle(lcm))
            spy.quit = 1;
    }

    pthread_join(pid, NULL);
    lcm_destroy(lcm);
    free(lcm_all);
    free(url);
    getopt_destroy(gopt);

    g_hash_table_destroy(spy.hashtable);

    pthread_mutex_destroy(&spy.mutex);
}
